//! Reset and Clock Control
#![deny(missing_docs)]

use crate::pwr::VoltageScale as Voltage;
use crate::stm32::rcc::cfgr::SW_A as SW;
use crate::stm32::rcc::cfgr::TIMPRE_A as TIMPRE;
use crate::stm32::rcc::d1ccipr::CKPERSEL_A as CKPERSEL;
use crate::stm32::rcc::d1cfgr::HPRE_A as HPRE;
use crate::stm32::rcc::pllckselr::PLLSRC_A as PLLSRC;
use crate::stm32::{rcc, RCC, SYSCFG};
use crate::time::Hertz;

/// This module configures the RCC unit to provide set frequencies for
/// the input to the SCGU `sys_ck`, the AMBA High-performace Busses
/// and Advanced eXtensible Interface bus `hclk`, the AMBA Peripheral
/// Busses `pclkN` and the periperal clock `per_ck`.
///
/// Check Fig 46 "Core and bus clock generation" in the reference
/// manual for information (p 336).
///
/// HSI is 64 MHz.
///

/// Extension trait that constrains the `RCC` peripheral
pub trait RccExt {
    /// Constrains the `RCC` peripheral so it plays nicely with the
    /// other abstractions
    fn constrain(self) -> Rcc;
}

impl RccExt for RCC {
    fn constrain(self) -> Rcc {
        Rcc {
            config: Config {
                hse: None,
                sys_ck: None,
                per_ck: None,
                rcc_hclk: None,
                rcc_pclk1: None,
                rcc_pclk2: None,
                rcc_pclk3: None,
                rcc_pclk4: None,
                pll1: PllConfig {
                    p_ck: None,
                    q_ck: None,
                    r_ck: None,
                },
                pll2: PllConfig {
                    p_ck: None,
                    q_ck: None,
                    r_ck: None,
                },
                pll3: PllConfig {
                    p_ck: None,
                    q_ck: None,
                    r_ck: None,
                },
            },
            rb: self,
        }
    }
}

/// Constrained RCC peripheral
///
/// Generated by calling `constrain` on the PAC's RCC peripheral.
///
/// ```rust
/// let dp = stm32::Peripherals::take().unwrap();
/// let rcc = dp.RCC.constrain();
/// ```
pub struct Rcc {
    config: Config,
    pub(crate) rb: RCC,
}

/// Core Clock Distribution and Reset (CCDR)
///
/// Generated when the RCC is frozen. The configuration of the Sys_Ck
/// `sys_ck`, CPU Clock `c_ck`, AXI peripheral clock `aclk`, AHB
/// clocks `hclk`, APB clocks `pclkN` and PLL outputs `pllN_X_ck` are
/// frozen. However the distribution of some clocks may still be
/// modified and peripherals enabled / reset by passing this object
/// to other implementations in this stack.
pub struct Ccdr {
    /// A record of the frozen core clock frequencies
    pub clocks: CoreClocks,
    /// AMBA High-performance Bus (AHB1) registers
    pub ahb1: AHB1,
    /// AMBA High-performance Bus (AHB2) registers
    pub ahb2: AHB2,
    /// AMBA High-performance Bus (AHB3) registers
    pub ahb3: AHB3,
    /// AMBA High-performance Bus (AHB4) registers
    pub ahb4: AHB4,
    /// Advanced Peripheral Bus 1L (APB1L) registers
    pub apb1l: APB1L,
    /// Advanced Peripheral Bus 1H (APB1H) registers
    pub apb1h: APB1H,
    /// Advanced Peripheral Bus 2 (APB2) registers
    pub apb2: APB2,
    /// Advanced Peripheral Bus 3 (APB3) registers
    pub apb3: APB3,
    /// Advanced Peripheral Bus 4 (APB4) registers
    pub apb4: APB4,
    /// RCC Domain 3 Kernel Clock Configuration Register
    pub d3ccipr: D3CCIPR,
    // Yes, it lives (locally)! We retain the right to switch most
    // PKSUs on the fly, to fine-tune PLL frequencies, and to enable /
    // reset peripherals.
    //
    // TODO: Remove this once all permitted RCC register accesses
    // after freeze are enumerated in this struct
    pub(crate) rb: RCC,
}

macro_rules! ahb_apb_generation {
    ($(($AXBn:ident, $AXBnENR:ident, $axbnenr:ident, $AXBnRSTR:ident, $axbnrstr:ident, $doc:expr)),+) => {
        $(
            #[doc=$doc]
            pub struct $AXBn {
                _0: (),
            }

            impl $AXBn {
                #[allow(unused)]
                pub (crate) fn enr(&mut self) -> &rcc::$AXBnENR {
                    // NOTE(unsafe) this proxy grants exclusive access to this register
                    unsafe { &(*RCC::ptr()).$axbnenr }
                }

                #[allow(unused)]
                pub (crate) fn rstr(&mut self) -> &rcc::$AXBnRSTR {
                    // NOTE(unsafe) this proxy grants exclusive access to this register
                    unsafe { &(*RCC::ptr()).$axbnrstr }
                }
            }
        )+
    }
}

ahb_apb_generation!(
    (
        AHB1,
        AHB1ENR,
        ahb1enr,
        AHB1RSTR,
        ahb1rstr,
        "AMBA High-performance Bus (AHB1) registers"
    ),
    (
        AHB2,
        AHB2ENR,
        ahb2enr,
        AHB2RSTR,
        ahb2rstr,
        "AMBA High-performance Bus (AHB2) registers"
    ),
    (
        AHB3,
        AHB3ENR,
        ahb3enr,
        AHB3RSTR,
        ahb3rstr,
        "AMBA High-performance Bus (AHB3) registers"
    ),
    (
        AHB4,
        AHB4ENR,
        ahb4enr,
        AHB4RSTR,
        ahb4rstr,
        "AMBA High-performance Bus (AHB4) registers"
    ),
    (
        APB1L,
        APB1LENR,
        apb1lenr,
        APB1LRSTR,
        apb1lrstr,
        "Advanced Peripheral Bus 1L (APB1L) registers"
    ),
    (
        APB1H,
        APB1HENR,
        apb1henr,
        APB1HRSTR,
        apb1hrstr,
        "Advanced Peripheral Bus 1H (APB1H) registers"
    ),
    (
        APB2,
        APB2ENR,
        apb2enr,
        APB2RSTR,
        apb2rstr,
        "Advanced Peripheral Bus 2 (APB2) registers"
    ),
    (
        APB3,
        APB3ENR,
        apb3enr,
        APB3RSTR,
        apb3rstr,
        "Advanced Peripheral Bus 3 (APB3) registers"
    ),
    (
        APB4,
        APB4ENR,
        apb4enr,
        APB4RSTR,
        apb4rstr,
        "Advanced Peripheral Bus 4 (APB4) registers"
    )
);

/// RCC Domain 3 Kernel Clock Configuration Register
pub struct D3CCIPR {
    _0: (),
}

impl D3CCIPR {
    pub(crate) fn kernel_ccip(&mut self) -> &rcc::D3CCIPR {
        unsafe { &(*RCC::ptr()).d3ccipr }
    }
}

const HSI: u32 = 64_000_000; // Hz
const CSI: u32 = 4_000_000; // Hz
const HSI48: u32 = 48_000_000; // Hz

/// Configuration of a Phase Lock Loop (PLL)
pub struct PllConfig {
    p_ck: Option<u32>,
    q_ck: Option<u32>,
    r_ck: Option<u32>,
}

/// Configuration of the core clocks
pub struct Config {
    hse: Option<u32>,
    sys_ck: Option<u32>,
    per_ck: Option<u32>,
    rcc_hclk: Option<u32>,
    rcc_pclk1: Option<u32>,
    rcc_pclk2: Option<u32>,
    rcc_pclk3: Option<u32>,
    rcc_pclk4: Option<u32>,
    pll1: PllConfig,
    pll2: PllConfig,
    pll3: PllConfig,
}

/// Setter defintion for pclk 1 - 4
macro_rules! pclk_setter {
    ($($name:ident: $pclk:ident,)+) => {
        $(
            /// Set the peripheral clock frequency for APB
            /// peripherals.
            pub fn $name<F>(mut self, freq: F) -> Self
            where
                F: Into<Hertz>,
            {
                self.config.$pclk = Some(freq.into().0);
                self
            }
        )+
    };
}

/// Setter definition for pll 1 - 3
macro_rules! pll_setter {
    ($($pll:ident: [ $($name:ident: $ck:ident,)+ ],)+) => {
        $(
            $(
                /// Set the target clock frequency for PLL output
                pub fn $name<F>(mut self, freq: F) -> Self
                where
                    F: Into<Hertz>,
                {
                    self.config.$pll.$ck = Some(freq.into().0);
                    self
                }
            )+
        )+
    };
}

impl Rcc {
    /// Uses HSE (external oscillator) instead of HSI (internal RC
    /// oscillator) as the clock source. Will result in a hang if an
    /// external oscillator is not connected or it fails to start.
    pub fn use_hse<F>(mut self, freq: F) -> Self
    where
        F: Into<Hertz>,
    {
        self.config.hse = Some(freq.into().0);
        self
    }

    /// Set input frequency to the SCGU
    pub fn sys_ck<F>(mut self, freq: F) -> Self
    where
        F: Into<Hertz>,
    {
        self.config.sys_ck = Some(freq.into().0);
        self
    }

    /// Set input frequency to the SCGU - ALIAS
    pub fn sysclk<F>(mut self, freq: F) -> Self
    where
        F: Into<Hertz>,
    {
        self.config.sys_ck = Some(freq.into().0);
        self
    }

    /// Set peripheral clock frequency
    pub fn per_ck<F>(mut self, freq: F) -> Self
    where
        F: Into<Hertz>,
    {
        self.config.per_ck = Some(freq.into().0);
        self
    }

    /// Set the peripheral clock frequency for AHB and AXI
    /// peripherals. There are several gated versions `rcc_hclk[1-4]`
    /// for different power domains, but they are all the same frequency
    pub fn hclk<F>(mut self, freq: F) -> Self
    where
        F: Into<Hertz>,
    {
        self.config.rcc_hclk = Some(freq.into().0);
        self
    }

    pclk_setter! {
        pclk1: rcc_pclk1,
        pclk2: rcc_pclk2,
        pclk3: rcc_pclk3,
        pclk4: rcc_pclk4,
    }

    pll_setter! {
        pll1: [
            pll1_p_ck: p_ck,
            pll1_q_ck: q_ck,
            pll1_r_ck: r_ck,
        ],
        pll2: [
            pll2_p_ck: p_ck,
            pll2_q_ck: q_ck,
            pll2_r_ck: r_ck,
        ],
        pll3: [
            pll3_p_ck: p_ck,
            pll3_q_ck: q_ck,
            pll3_r_ck: r_ck,
        ],
    }
}

/// Divider calculator for pclk 1 - 4
///
/// Also calulate tim[xy]_ker_clk if there are timers on this bus
macro_rules! ppre_calculate {
    ($(($ppre:ident, $bits:ident): ($self: ident, $hclk: ident,
                                    $pclk: ident, $max: ident
                                    $(,$rcc_tim_ker_clk:ident, $timpre:ident)*),)+) => {
        $(
            // Get intended rcc_pclkN frequency
            let $pclk: u32 = $self.config
                .$pclk
                .unwrap_or_else(|| core::cmp::min($max, $hclk / 2));

            // Calculate suitable divider
            let ($bits, $ppre) = match ($hclk + $pclk - 1) / $pclk
            {
                0 => unreachable!(),
                1 => (0b000, 1 as u8),
                2 => (0b100, 2),
                3..=5 => (0b101, 4),
                6..=11 => (0b110, 8),
                _ => (0b111, 16),
            };

            // Calculate real APBn clock
            let $pclk = $hclk / u32::from($ppre);

            // Check in range
            assert!($pclk <= $max);

            $(
                let $rcc_tim_ker_clk = match ($bits, &$timpre)
                {
                    (0b101, TIMPRE::DEFAULTX2) => $hclk / 2,
                    (0b110, TIMPRE::DEFAULTX4) => $hclk / 2,
                    (0b110, TIMPRE::DEFAULTX2) => $hclk / 4,
                    (0b111, TIMPRE::DEFAULTX4) => $hclk / 4,
                    (0b111, TIMPRE::DEFAULTX2) => $hclk / 8,
                    _ => $hclk,
                };
            )*
        )+
    };
}

/// Setup PFD input frequency and VCO output frequency
///
macro_rules! vco_setup {
    // VCOL, highest PFD frequency, highest VCO frequency
    (NORMAL: $pllsrc:ident, $output:ident,
     $rcc:ident, $pllXvcosel:ident, $pllXrge:ident $(,$pll1_p:ident)*) => {{
         // Input divisor, resulting in a reference clock in the
         // range 1 to 2 MHz. Choose the highest reference clock
         let pll_x_m = ($pllsrc + 1_999_999) / 2_000_000;

         assert!(pll_x_m < 64);

         // Calculate resulting reference clock
         let ref_x_ck = $pllsrc / pll_x_m;
         assert!(ref_x_ck >= 1_000_000 && ref_x_ck <= 2_000_000);

         // VCO output frequency. Choose the highest VCO frequency
         let vco_min = 150_000_000;
         let vco_max = 420_000_000;

         // Macro-based selection
         let pll_x_p = match true {
             $(
                 // Specific to PLL1
                 true => {
                     let $pll1_p = if $output > vco_max / 2 {
                         1
                     } else {
                         ((vco_max / $output) | 1) - 1 // Must be even or unity
                     };
                     $pll1_p
                 },
             )*
             // Specific to PLL2/3
             _ => if $output > vco_max / 2 {
                 1
             } else {
                 vco_max / $output
             }
         };

         // Calcuate VCO output
         let vco_ck = $output * pll_x_p;

         assert!(pll_x_p <= 128);
         assert!(vco_ck >= vco_min);
         assert!(vco_ck <= vco_max);

         // Configure VCO
         $rcc.pllcfgr.modify(|_, w| {
             w.$pllXvcosel()
                 .medium_vco() // 150 - 420MHz Medium VCO
                 .$pllXrge()
                 .range1() // ref_x_ck is 1 - 2 MHz
         });

         (ref_x_ck, pll_x_m, pll_x_p, vco_ck)
     }};
}

macro_rules! pll_setup {
    ($pll_setup:ident: ($pllXvcosel:ident, $pllXrge:ident, $pllXfracen:ident,
                   $pllXdivr:ident, $divnX:ident, $divmX:ident,
                   OUTPUTS: [ $($CK:ident:
                                ($div:ident, $diven:ident, $DD:tt $(,$unsafe:ident)*)),+ ]
                   $(,$pll1_p:ident)*
    )) => {
        /// PLL Setup
        /// Returns (Option(pllX_p_ck), Option(pllX_q_ck), Option(pllX_r_ck))
        fn $pll_setup(
            &self,
            rcc: &RCC,
            pll: &PllConfig,
        ) -> (Option<Hertz>, Option<Hertz>, Option<Hertz>) {
            // PLL sourced from either HSE or HSI
            let pllsrc = self.config.hse.unwrap_or(HSI);
            assert!(pllsrc > 0);

            // PLL output
            match pll.p_ck {
                Some(output) => {
                    // Use the Medium Range VCO with 1 - 2 MHz input
                    let (ref_x_ck, pll_x_m, pll_x_p, vco_ck) = {
                        vco_setup! { NORMAL: pllsrc, output, rcc,
                                     $pllXvcosel, $pllXrge $(, $pll1_p)* }
                    };

                    // Feedback divider. Integer only
                    let pll_x_n = vco_ck / ref_x_ck;

                    // Write dividers
                    rcc.pllckselr.modify(|_, w| {
                        w.$divmX().bits(pll_x_m as u8) // ref prescaler
                    });
                    // unsafe as not all values are permitted: see RM0433
                    assert!(pll_x_n >= 4);
                    assert!(pll_x_n <= 512);
                    rcc.$pllXdivr
                        .modify(|_, w| unsafe { w.$divnX().bits((pll_x_n - 1) as u16) });

                    // Configure PLL
                    rcc.pllcfgr.modify(|_, w| {
                        w.$pllXfracen().reset() // No FRACN
                    });

                    // Calulate additional output dividers
                    let pll_x_q = match pll.q_ck {
                        Some(ck) => (vco_ck + ck - 1) / ck,
                        None => 0
                    };
                    let pll_x_r = match pll.r_ck {
                        Some(ck) => (vco_ck + ck - 1) / ck,
                        None => 0
                    };
                    let dividers = (pll_x_p, pll_x_q, pll_x_r);

                    // Setup and return output clocks
                    ($(
                        // Enable based on config
                        match pll.$CK {
                            Some(_) => {
                                // Setup divider
                                rcc.$pllXdivr
                                    .modify(|_, w| $($unsafe)* {
                                        w.$div().bits((dividers.$DD - 1) as u8)
                                    });

                                rcc.pllcfgr.modify(|_, w| w.$diven().enabled());
                                Some(Hertz(ref_x_ck * pll_x_n / dividers.$DD))
                            }
                            None => {
                                rcc.pllcfgr.modify(|_, w| w.$diven().disabled());
                                None
                            }
                        },
                    )+)
                },
                None => {
                    assert!(pll.q_ck.is_none(), "Must set PLL P clock for Q clock to take effect!");
                    assert!(pll.r_ck.is_none(), "Must set PLL P clock for R clock to take effect!");
                    (None, None, None)
                }
            }
        }
    };
}

impl Rcc {
    pll_setup! {
    pll1_setup: (pll1vcosel, pll1rge, pll1fracen, pll1divr, divn1, divm1,
                 OUTPUTS: [
                      // unsafe as not all values are permitted: see RM0433
                     p_ck: (divp1, divp1en, 0, unsafe),
                     q_ck: (divq1, divq1en, 1),
                     r_ck: (divr1, divr1en, 2) ],
                 pll1_p)
    }
    pll_setup! {
    pll2_setup: (pll2vcosel, pll2rge, pll2fracen, pll2divr, divn2, divm2,
                 OUTPUTS: [
                     p_ck: (divp2, divp2en, 0),
                     q_ck: (divq2, divq2en, 1),
                     r_ck: (divr2, divr2en, 2)])
    }
    pll_setup! {
    pll3_setup: (pll3vcosel, pll3rge, pll3fracen, pll3divr, divn3, divm3,
                 OUTPUTS: [
                     p_ck: (divp3, divp3en, 0),
                     q_ck: (divq3, divq3en, 1),
                     r_ck: (divr3, divr3en, 2)])
    }

    fn flash_setup(rcc_aclk: u32, vos: Voltage) {
        use crate::stm32::FLASH;
        let rcc_aclk_mhz = rcc_aclk / 1_000_000;

        // See RM0433 Table 13. FLASH recommended number of wait
        // states and programming delay
        let (wait_states, progr_delay) = match vos {
            // VOS 1 range VCORE 1.15V - 1.26V
            Voltage::Scale0 | Voltage::Scale1 => match rcc_aclk_mhz {
                0..=69 => (0, 0),
                70..=139 => (1, 1),
                140..=184 => (2, 1),
                185..=209 => (2, 2),
                210..=224 => (3, 2),
                _ => (7, 3),
            },
            // VOS 2 range VCORE 1.05V - 1.15V
            Voltage::Scale2 => match rcc_aclk_mhz {
                0..=54 => (0, 0),
                55..=109 => (1, 1),
                110..=164 => (2, 1),
                165..=224 => (3, 2),
                225 => (4, 2),
                _ => (7, 3),
            },
            // VOS 3 range VCORE 0.95V - 1.05V
            Voltage::Scale3 => match rcc_aclk_mhz {
                0..=44 => (0, 0),
                45..=89 => (1, 1),
                90..=134 => (2, 1),
                135..=179 => (3, 2),
                180..=224 => (4, 2),
                _ => (7, 3),
            },
        };

        let flash = unsafe { &(*FLASH::ptr()) };
        // Adjust flash wait states
        flash.acr.write(|w| unsafe {
            w.wrhighfreq().bits(progr_delay).latency().bits(wait_states)
        });
        while flash.acr.read().latency().bits() != wait_states {}
    }

    /// Setup sys_ck
    /// Returns sys_ck frequency, and a pll1_p_ck
    fn sys_ck_setup(&self) -> (Hertz, Option<u32>, bool) {
        // Compare available with wanted clocks
        let srcclk = self.config.hse.unwrap_or(HSI); // Available clocks
        let sys_ck = self.config.sys_ck.unwrap_or(srcclk);

        // The requested system clock is not the immediately available
        // HSE/HSI clock. Perhaps there are other ways of obtaining
        // the requested system clock (such as `HSIDIV`) but we will
        // ignore those for now.
        if sys_ck != srcclk {
            // Therefore we must use pll1_p_ck
            let pll1_p_ck = match self.config.pll1.p_ck {
                Some(p_ck) => {
                    assert!(p_ck == sys_ck,
                            "Error: Cannot set pll1_p_ck independently as it must be used to generate sys_ck");
                    Some(p_ck)
                }
                None => Some(sys_ck),
            };

            (Hertz(sys_ck), pll1_p_ck, true)
        } else {
            // sys_ck is derived directly from a source clock
            // (HSE/HSI). pll1_p_ck can be as requested
            (Hertz(sys_ck), self.config.pll1.p_ck, false)
        }
    }

    /// Setup traceclk
    /// Returns a pll1_r_ck
    fn traceclk_setup(
        &self,
        sys_use_pll1_p: bool,
        pll1_p_ck: Option<u32>,
    ) -> Option<u32> {
        let pll1_r_ck = match (sys_use_pll1_p, self.config.pll1.r_ck) {
            // pll1_p_ck selected as system clock but pll1_r_ck not
            // set. The traceclk mux is synchronous with the system
            // clock mux, but has pll1_r_ck as an input. In order to
            // keep traceclk running, we force a pll1_r_ck.
            (true, None) => Some(pll1_p_ck.unwrap() / 2),
            // Either pll1 not selected as system clock, free choice
            // of pll1_r_ck. Or pll1 is selected, assume user has set
            // a suitable pll1_r_ck frequency.
            _ => self.config.pll1.r_ck,
        };

        pll1_r_ck
    }

    /// Freeze the core clocks, returning a Core Clocks Distribution
    /// and Reset (CCDR) object.
    ///
    /// `syscfg` is required to enable the I/O compensation cell.
    pub fn freeze(self, vos: Voltage, syscfg: &SYSCFG) -> Ccdr {
        let rcc = &self.rb;

        // We do not reset RCC here. This routine must assert when
        // the previous state of the RCC peripheral is unacceptable.

        // sys_ck from PLL if needed, else HSE or HSI
        let (sys_ck, pll1_p_ck, sys_use_pll1_p) = self.sys_ck_setup();

        // Configure traceclk from PLL if needed
        let pll1_r_ck = self.traceclk_setup(sys_use_pll1_p, pll1_p_ck);

        // Configure PLL1
        let pll1_config = PllConfig {
            p_ck: pll1_p_ck,
            q_ck: self.config.pll1.q_ck,
            r_ck: pll1_r_ck,
        };
        let (pll1_p_ck, pll1_q_ck, pll1_r_ck) =
            self.pll1_setup(rcc, &pll1_config);
        // Configure PLL2
        let (pll2_p_ck, pll2_q_ck, pll2_r_ck) =
            self.pll2_setup(rcc, &self.config.pll2);
        // Configure PLL3
        let (pll3_p_ck, pll3_q_ck, pll3_r_ck) =
            self.pll3_setup(rcc, &self.config.pll3);

        // hsi_ck = HSI. This routine does not support HSIDIV != 1. To
        // do so it would need to ensure all PLLxON bits are clear
        // before changing the value of HSIDIV
        let hsi = HSI;
        assert!(rcc.cr.read().hsion().is_on(), "HSI oscillator must be on!");
        assert!(rcc.cr.read().hsidiv().is_div1());

        let csi = CSI;
        let hsi48 = HSI48;

        // per_ck from HSI by default
        let (per_ck, ckpersel) =
            match (self.config.per_ck == self.config.hse, self.config.per_ck) {
                (true, Some(hse)) => (hse, CKPERSEL::HSE), // HSE
                (_, Some(CSI)) => (csi, CKPERSEL::CSI),    // CSI
                _ => (hsi, CKPERSEL::HSI),                 // HSI
            };

        // D1 Core Prescaler
        // Set to 1
        let d1cpre_bits = 0;
        let d1cpre_div = 1;
        let sys_d1cpre_ck = sys_ck.0 / d1cpre_div;

        // Timer prescaler selection
        let timpre = TIMPRE::DEFAULTX2;

        // Refer to part datasheet "General operating conditions"
        // table for (rev V). We do not assert checks for earlier
        // revisions which may have lower limits.
        let (sys_d1cpre_ck_max, rcc_hclk_max, pclk_max) = match vos {
            Voltage::Scale0 => (480_000_000, 240_000_000, 120_000_000),
            Voltage::Scale1 => (400_000_000, 200_000_000, 100_000_000),
            Voltage::Scale2 => (300_000_000, 150_000_000, 75_000_000),
            _ => (200_000_000, 100_000_000, 50_000_000),
        };

        // Check resulting sys_d1cpre_ck
        assert!(sys_d1cpre_ck <= sys_d1cpre_ck_max);

        // Get ideal AHB clock
        let rcc_hclk = self.config.rcc_hclk.unwrap_or(sys_d1cpre_ck / 2);
        assert!(rcc_hclk <= rcc_hclk_max);

        // Estimate divisor
        let (hpre_bits, hpre_div) =
            match (sys_d1cpre_ck + rcc_hclk - 1) / rcc_hclk {
                0 => unreachable!(),
                1 => (HPRE::DIV1, 1),
                2 => (HPRE::DIV2, 2),
                3..=5 => (HPRE::DIV4, 4),
                6..=11 => (HPRE::DIV8, 8),
                12..=39 => (HPRE::DIV16, 16),
                40..=95 => (HPRE::DIV64, 64),
                96..=191 => (HPRE::DIV128, 128),
                192..=383 => (HPRE::DIV256, 256),
                _ => (HPRE::DIV512, 512),
            };

        // Calculate real AXI and AHB clock
        let rcc_hclk = sys_d1cpre_ck / hpre_div;
        assert!(rcc_hclk <= rcc_hclk_max);

        // Calculate ppreN dividers and real rcc_pclkN frequencies
        ppre_calculate! {
            (ppre1, ppre1_bits):
                (self, rcc_hclk, rcc_pclk1, pclk_max, rcc_timx_ker_ck, timpre),
            (ppre2, ppre2_bits):
                (self, rcc_hclk, rcc_pclk2, pclk_max, rcc_timy_ker_ck, timpre),
            (ppre3, ppre3_bits): (self, rcc_hclk, rcc_pclk3, pclk_max),
            (ppre4, ppre4_bits): (self, rcc_hclk, rcc_pclk4, pclk_max),
        }

        // Start switching clocks here! ----------------------------------------

        // Flash setup
        Self::flash_setup(sys_d1cpre_ck, vos);

        // Ensure CSI is on and stable
        rcc.cr.modify(|_, w| w.csion().on());
        while rcc.cr.read().csirdy().is_not_ready() {}

        // Ensure HSI48 is on and stable
        rcc.cr.modify(|_, w| w.hsi48on().on());
        while rcc.cr.read().hsi48rdy().is_not_ready() {}

        // HSE
        let hse_ck = match self.config.hse {
            Some(hse) => {
                // Ensure HSE is on and stable
                rcc.cr.modify(|_, w| w.hseon().on().hsebyp().not_bypassed());
                while rcc.cr.read().hserdy().is_not_ready() {}

                Some(Hertz(hse))
            }
            None => None,
        };

        // PLL
        let pllsrc = if self.config.hse.is_some() {
            PLLSRC::HSE
        } else {
            PLLSRC::HSI
        };
        rcc.pllckselr.modify(|_, w| w.pllsrc().variant(pllsrc));

        // PLL1
        if pll1_p_ck.is_some() {
            // Enable PLL and wait for it to stabilise
            rcc.cr.modify(|_, w| w.pll1on().on());
            while rcc.cr.read().pll1rdy().is_not_ready() {}
        }

        // PLL2
        if pll2_p_ck.is_some() {
            // Enable PLL and wait for it to stabilise
            rcc.cr.modify(|_, w| w.pll2on().on());
            while rcc.cr.read().pll2rdy().is_not_ready() {}
        }

        // PLL3
        if pll3_p_ck.is_some() {
            // Enable PLL and wait for it to stabilise
            rcc.cr.modify(|_, w| w.pll3on().on());
            while rcc.cr.read().pll3rdy().is_not_ready() {}
        }

        // Core Prescaler / AHB Prescaler / APB3 Prescaler
        rcc.d1cfgr.modify(|_, w| unsafe {
            w.d1cpre()
                .bits(d1cpre_bits)
                .d1ppre() // D1 contains APB3
                .bits(ppre3_bits)
                .hpre()
                .variant(hpre_bits)
        });
        // Ensure core prescaler value is valid before future lower
        // core voltage
        while rcc.d1cfgr.read().d1cpre().bits() != d1cpre_bits {}

        // APB1 / APB2 Prescaler
        rcc.d2cfgr.modify(|_, w| unsafe {
            w.d2ppre1() // D2 contains APB1
                .bits(ppre1_bits)
                .d2ppre2() // D2 also contains APB2
                .bits(ppre2_bits)
        });

        // APB4 Prescaler
        rcc.d3cfgr.modify(|_, w| unsafe {
            w.d3ppre() // D3 contains APB4
                .bits(ppre4_bits)
        });

        // Peripheral Clock (per_ck)
        rcc.d1ccipr.modify(|_, w| w.ckpersel().variant(ckpersel));

        // Set timer clocks prescaler setting
        rcc.cfgr.modify(|_, w| w.timpre().variant(timpre));

        // Select system clock source
        let swbits = match (sys_use_pll1_p, self.config.hse.is_some()) {
            (true, _) => SW::PLL1 as u8,
            (false, true) => SW::HSE as u8,
            _ => SW::HSI as u8,
        };
        rcc.cfgr.modify(|_, w| unsafe { w.sw().bits(swbits) });
        while rcc.cfgr.read().sws().bits() != swbits {}

        // IO compensation cell - Requires CSI clock and SYSCFG
        assert!(rcc.cr.read().csirdy().is_ready());
        rcc.apb4enr.modify(|_, w| w.syscfgen().enabled());

        // Enable the compensation cell, using back-bias voltage code
        // provide by the cell.
        syscfg.cccsr.modify(|_, w| {
            w.en().set_bit().cs().clear_bit().hslv().clear_bit()
        });
        while syscfg.cccsr.read().ready().bit_is_clear() {}

        // Return frozen clock configuration
        Ccdr {
            ahb1: AHB1 { _0: () },
            ahb2: AHB2 { _0: () },
            ahb3: AHB3 { _0: () },
            ahb4: AHB4 { _0: () },
            apb1l: APB1L { _0: () },
            apb1h: APB1H { _0: () },
            apb2: APB2 { _0: () },
            apb3: APB3 { _0: () },
            apb4: APB4 { _0: () },
            clocks: CoreClocks {
                hclk: Hertz(rcc_hclk),
                pclk1: Hertz(rcc_pclk1),
                pclk2: Hertz(rcc_pclk2),
                pclk3: Hertz(rcc_pclk3),
                pclk4: Hertz(rcc_pclk4),
                ppre1,
                ppre2,
                ppre3,
                ppre4,
                csi_ck: Some(Hertz(csi)),
                hsi_ck: Some(Hertz(hsi)),
                hsi48_ck: Some(Hertz(hsi48)),
                per_ck: Some(Hertz(per_ck)),
                hse_ck,
                pll1_p_ck,
                pll1_q_ck,
                pll1_r_ck,
                pll2_p_ck,
                pll2_q_ck,
                pll2_r_ck,
                pll3_p_ck,
                pll3_q_ck,
                pll3_r_ck,
                timx_ker_ck: Hertz(rcc_timx_ker_ck),
                timy_ker_ck: Hertz(rcc_timy_ker_ck),
                sys_ck,
                c_ck: Hertz(sys_d1cpre_ck),
            },
            d3ccipr: D3CCIPR { _0: () },
            rb: self.rb,
        }
    }
}

/// Frozen core clock frequencies
///
/// The existence of this value indicates that the core clock
/// configuration can no longer be changed
#[derive(Clone, Copy)]
pub struct CoreClocks {
    hclk: Hertz,
    pclk1: Hertz,
    pclk2: Hertz,
    pclk3: Hertz,
    pclk4: Hertz,
    ppre1: u8,
    ppre2: u8,
    ppre3: u8,
    ppre4: u8,
    csi_ck: Option<Hertz>,
    hsi_ck: Option<Hertz>,
    hsi48_ck: Option<Hertz>,
    per_ck: Option<Hertz>,
    hse_ck: Option<Hertz>,
    pll1_p_ck: Option<Hertz>,
    pll1_q_ck: Option<Hertz>,
    pll1_r_ck: Option<Hertz>,
    pll2_p_ck: Option<Hertz>,
    pll2_q_ck: Option<Hertz>,
    pll2_r_ck: Option<Hertz>,
    pll3_p_ck: Option<Hertz>,
    pll3_q_ck: Option<Hertz>,
    pll3_r_ck: Option<Hertz>,
    timx_ker_ck: Hertz,
    timy_ker_ck: Hertz,
    sys_ck: Hertz,
    c_ck: Hertz,
}

/// Getters for pclk and ppre
macro_rules! pclk_ppre_getter {
    ($(($pclk:ident, $ppre:ident),)+) => {
        $(
            /// Returns the frequency of the APBn
            pub fn $pclk(&self) -> Hertz {
                self.$pclk
            }
            /// Returns the prescaler of the APBn
            pub fn $ppre(&self) -> u8 {
                self.$ppre
            }
        )+
    };
}

/// Getters for optional clocks
macro_rules! optional_ck_getter {
    ($($opt_ck:ident,)+) => {
        $(
            /// Returns the frequency of optional clock $opt_ck
            pub fn $opt_ck(&self) -> Option<Hertz> {
                self.$opt_ck
            }
        )+
    };
}

/// Getters for pll clocks
macro_rules! pll_getter {
    ($($pll_ck:ident,)+) => {
        $(
            /// Returns the frequency of the PLLx output
            pub fn $pll_ck(&self) -> Option<Hertz> {
                self.$pll_ck
            }
        )+
    };
}

impl CoreClocks {
    /// Returns the frequency of AHB1,2,3 busses
    pub fn hclk(&self) -> Hertz {
        self.hclk
    }

    /// Returns the frequency of the AXI bus
    pub fn aclk(&self) -> Hertz {
        self.hclk // Same as HCLK
    }

    pclk_ppre_getter! {
        (pclk1, ppre1),
        (pclk2, ppre2),
        (pclk3, ppre3),
        (pclk4, ppre4),
    }

    optional_ck_getter! {
        csi_ck,
        hsi_ck,
        hsi48_ck,
        per_ck,
        hse_ck,
    }

    pll_getter! {
        pll1_p_ck,
        pll1_q_ck,
        pll1_r_ck,
        pll2_p_ck,
        pll2_q_ck,
        pll2_r_ck,
        pll3_p_ck,
        pll3_q_ck,
        pll3_r_ck,
    }

    /// Returns the input frequency to the SCGU
    pub fn sys_ck(&self) -> Hertz {
        self.sys_ck
    }

    /// Returns the input frequency to the SCGU - ALIAS
    pub fn sysclk(&self) -> Hertz {
        self.sys_ck
    }

    /// Returns the CK_INT frequency for timers on APB1
    pub fn timx_ker_ck(&self) -> Hertz {
        self.timx_ker_ck
    }

    /// Returns the CK_INT frequency for timers on APB2
    pub fn timy_ker_ck(&self) -> Hertz {
        self.timy_ker_ck
    }

    /// Returns the core frequency
    pub fn c_ck(&self) -> Hertz {
        self.c_ck
    }
}
